/*************************************************
 *  Object:       Tric! A flying Ricotta.
 *  Authors:      Teo (matteo.piva-gmail.com)
                  Carletto (carlo.checchi-gmail.com)
*************************************************/

#include "config.h"
#include "sensors.h"
#include "pid.h"
#include "motors.h"
#include "rx.h"
#include "util.h"


// Variables - Main Thread
unsigned short THREAD_counts;   // Number of timeslices gone (1ms each)
unsigned short THREAD_ready;    // Next timeslice started flag
unsigned short RX_counts;
unsigned short MOTORS_speed[4];
unsigned short RX_interruptTimer[7];
unsigned short THREAD_emergencyLanding;
float SENSORS_acc[3];
float SENSORS_gyro[3];
float SENSORS_angle[3];
float PID_output[3];
float RX_values[7];


/* MAIN LOOP
 */
void main() {
    #ifdef DEBUG
    char roll_string[8];
    char pitch_string[8];
    char yaw_string[8];
    char motor1_string[8];
    char motor2_string[8];
    char motor3_string[8];
    char servo_string[8];
    #endif

    /* Registers initialization */
    ADCON1 = 0x0F;    // Set A/D Disabled
    //ADCON0 = 0x01;    // Set A/D Enabled
    //ADCON1 = 0x0E;    // Set A/D AN0 Enabled, all others Digital I/O
    CMCON  = 0xCF;    // Set Comparator Disabled

    /* Pinout direction assignment */
    TRISA = 0b00000001;
    TRISB = 0b00000100;
    TRISC = 0b00000000;

    /* Startup pinout conditions*/
    LED_STATUS1 = 1;
    LED_STATUS2 = 0;
    LED_STATUS3 = 0;
    LED_AUX = 0;
    FPV_ENABLE = 0;
    BEEPER = 0;
    MOTORS_M1 = 1;
    MOTORS_M2 = 1;
    MOTORS_M3 = 1;
    MOTORS_MS = 1;
    Delay_ms(1000);


    /* Periphals initialization */
    #ifdef DEBUG
    Soft_UART_Init(&PORTB, 6, 7, 115200, 0);
    #endif
    SENSORS_init();
    MOTORS_M1 = 1;
    MOTORS_M2 = 1;
    MOTORS_M3 = 1;
    MOTORS_MS = 1;
    Delay_ms(1000);
    LED_STATUS1 = 0;
    LED_STATUS2 = 1;
    

    /* Timer0 initialization */
    T0CON.T0PS0 = 0;     // Prescaler 1:64
    T0CON.T0PS1 = 1;     // Prescaler 1:64
    T0CON.T0PS2 = 1;     // Prescaler 1:64
    T0CON.PSA = 1;       // Timer Clock Source is from Prescaler
    T0CON.T0CS = 0;      // Prescaler gets clock from FCPU
    T0CON.T08BIT = 0;    // Disable 8 BIT MODE
    TMR0L = 0;           // TMR0 preset
    TMR0H = 0;           // TMR0 preset
    T0CON.TMR0ON = 0;    // Timer0 start/stop


    /* Timer1 initialization */
    T1CON.T1CKPS1 = 0;        // Prescaler 1:0
    T1CON.T1CKPS0 = 0;        // Prescaler 1:0   --> 0,33uS per tick
    TMR1H = 0xD1;             // High byte timer
    TMR1L = 0x80;             // Low  byte timer
    PIR1.TMR1IF = 0 ;         // Clear timer 1 interrupt flag
    T1CON.TMR1ON = 0;         // Timer1 start/stop

    
    /* Interrupts */
    INTCON.GIE = 1 ;         // Enable global interrupts
    INTCON.PEIE = 1 ;        // Enable Peripheral Interrupt
    PIE1.TMR1IE = 1  ;       // Enable Timer1 Interrupt
    PIR1.TMR1IF = 0 ;        // Clear Timer1 Interrupt flag
    INTCON.TMR0IE = 0;       // Enable Timer0 Interrupt
    INTCON.TMR0IF = 0;       // Clear Timer0 Interrupt flag
    INTCON2.INTEDG2 = 1;     // Set INT2 on rising edge
    INTCON3.INT2IE = 1;      // Enable INT2 Interrupt
    INTCON3.INT2IF = 0;      // Clear INT2 Interrupt flag


    /* Variables initialization */
    THREAD_counts = 0;
    THREAD_ready = 0;
    RX_counts=0;
    THREAD_emergencyLanding = 0;
    MOTORS_speed[0] = 55;
    MOTORS_speed[1] = 55;
    MOTORS_speed[2] = 55;
    MOTORS_speed[3] = 55;

    /* Startup */
    #ifndef NOESCCONFIG
    MOTORS_esc_config();
    #endif
    MOTORS_esc_startup();
    LED_STATUS2 = 0;
    T1CON.TMR1ON = 1;
    /*while (1){
    MOTORS_M1=0;
    MOTORS_M2=0;
    MOTORS_M3=0;
    MOTORS_MS=0;
    Delay_ms(1);
    MOTORS_update(&MOTORS_speed);
    MOTORS_M1=1;
    MOTORS_M2=1;
    MOTORS_M3=1;
    MOTORS_MS=1;
    Delay_ms(20);
    }*/

    while (1) {
        if(THREAD_ready){
             switch(THREAD_counts){
                  case 1:
                      LED_STATUS1 = !LED_STATUS1;
                      break;

                  case 11:
                       //UTIL_initCheckBattery();
                       break;
                  case 13:
                       //THREAD_emergencyLanding = UTIL_checkBattery();
                       break;
                       
                  case 20:
                       #ifndef DEBUG
                       MOTORS_M1 = 0;
                       MOTORS_M2 = 0;
                       MOTORS_M3 = 0;
                       MOTORS_MS = 0;
                       #endif
                       // Intentionally left without break

                  case 0:
                       #ifdef DEBUG
                       IntToStr(SENSORS_angle[0], roll_string);
                       IntToStr(SENSORS_angle[1], pitch_string);
                       IntToStr(SENSORS_angle[2], yaw_string);
                       IntToStr(MOTORS_speed[0], motor1_string);
                       IntToStr(MOTORS_speed[1], motor2_string);
                       IntToStr(MOTORS_speed[2], motor3_string);
                       IntToStr(MOTORS_speed[3], servo_string);
                       Soft_UART_Write_Text("21,");
                       #endif
                  case 2:
                       #ifdef DEBUG
                       Soft_UART_Write_Text(&roll_string);
                       Soft_UART_Write_Text(",");
                       #endif
                  case 4:
                       #ifdef DEBUG
                       Soft_UART_Write_Text(&pitch_string);
                       Soft_UART_Write_Text(",");
                       #endif
                  case 6:
                       #ifdef DEBUG
                       Soft_UART_Write_Text(&yaw_string);
                       Soft_UART_Write_Text(",");
                       #endif
                  case 8:
                       #ifdef DEBUG
                       Soft_UART_Write_Text(&motor1_string);
                       Soft_UART_Write_Text(",");
                       #endif
                  case 10:
                       #ifdef DEBUG
                       Soft_UART_Write_Text(&motor2_string);
                       Soft_UART_Write_Text(",");
                       #endif
                  case 12:
                       #ifdef DEBUG
                       Soft_UART_Write_Text(&motor3_string);
                       Soft_UART_Write_Text(",");
                       #endif
                  case 14:
                       #ifdef DEBUG
                       Soft_UART_Write_Text(&servo_string);
                       Soft_UART_Write_Text(10);
                       #endif
                  case 16:
                  case 18:
                      //RX_processValues(&RX_interruptTimer, &RX_values);
                      //SENSORS_read(&SENSORS_acc, &SENSORS_gyro);
                      //SENSORS_process(&SENSORS_acc, &SENSORS_gyro, &SENSORS_angle);
                      //PID_update(&RX_values, &SENSORS_angle, &PID_output);
                      //PID_applyToMotors(&PID_output, &MOTORS_speed);
                      RX_applyThrottleToMotors(&RX_values, &MOTORS_speed);
                      break;

                  case 21:
                      MOTORS_update(&MOTORS_speed);
                      THREAD_counts = 0;
                      break;
             }
             THREAD_ready = 0;
        }
    }
}

void interrupt() {

     // Timer1 clock cycle (1ms)
     if(PIR1.TMR1IF) {
          TMR1H = 0xD1;
          TMR1L = 0x80;
          PIR1.TMR1IF = 0;
          THREAD_counts++;
          THREAD_ready = 1;
     }
     
     // INT2 rising edge
     /*if (INTCON3.INT2IF) {
         T0CON.TMR0ON = 0;
         if (TMR0H) RX_interruptTimer[RX_counts] = 0xFF;
         else RX_interruptTimer[RX_counts] = TMR0L;
         if (RX_counts == 6) RX_counts = 0;
         else RX_counts++;
         TMR0H = 0; TMR0L = 0;
         INTCON3.INT2IF = 0;
         T0CON.TMR0ON = 1;
         LED_STATUS3 = !LED_STATUS3;
     }*/
     
 }
